#define DBGLSV
module cmfd2g
    use mtbl2g
!    use geom, only: nxy, nz
    implicit none
    real(8), private, pointer, dimension(:,:,:)     :: phi
    real(8), private, pointer, dimension(:,:)       :: psi, psip
    type(mb2g), private :: mtcmfd, mtcmfdorg
    real(8), private, pointer, dimension(:,:,:,:)   :: dtilde, dhat
    integer, private :: maxout, maxin 
    real(8), private :: ccout, ccin
    integer, private :: nx, ny, nxy, nz
    logical, private :: l1gcmfd, ifwldt
    real(8), private, pointer, dimension(:,:,:)       :: dif, rmv, nuf, mcphi
    real(8), private, pointer, dimension(:,:,:,:)     :: sct, mcj
    real(8), private :: k_eff
    logical, private, pointer, dimension(:,:,:,:)       :: vacmesh
    real(8), private, pointer, dimension(:,:)           :: psiacc
    
    
    
contains

subroutine resetpsi_cmfd2g
    psiacc=0;
endsubroutine

subroutine initcmfd2g
    use allocs
    use bicg2g, only : mallocbicg2g
    implicit none
    include "pspec.FI"
    include "xsec.FI"
    include "hmgz.FI"
    
    nx=ncmx;ny=ncmy;nxy=nx*ny;nz=1
!    nx=ncmx;ny=ncmy;nxy=nreal;nz=1
    call dmalloc0(phi,1,2,0,nxy,0,1)
    call dmalloc(psi,nxy,1)
    call dmalloc(psip,nxy,1)
    call dmalloc(dtilde,4,ng2,nx,ny)
    call dmalloc(dhat,4,ng2,nx,ny)
    call newmb2g(mtcmfd,nxy,nx,ny, 1)
    call newmb2g(mtcmfdorg,nxy,nx,ny, 1)
    call mallocbicg2g(nxy,nx,ny,nz)
    call dmalloc(vacmesh, ng2, nx, ny, 1)
    call dmalloc(psiacc, nxy, 1)
    
    ifwldt = lwldt
!    if (ifwldt) print *, "Wielandt acceleration turned on!"
    if (gc2(ng) .eq. 1) l1gcmfd = .true.
    
    psip=1;

    maxout = max_out
    maxin  = max_in
    ccout = err_out
    ccin  = err_in
    
    if (lmgcmfd) then
        dif=>hdif2
        rmv=>hrmv2
        nuf=>hnuf2
        sct=>hmsct2
        mcphi=>hphi2
        mcj=>hj2
    else
        dif=>hdif
        rmv=>hrmv
        nuf=>hnuf
        sct=>hmsct
        mcphi=>hphi
        mcj=>hj
    endif
!    phi=1.
    open (5001, file="vacmesh.txt", status='unknown')
endsubroutine

subroutine solcmfd2g
    include "pspec.FI"
    include "xsec.FI"
    include "hmgz.FI"
    integer :: iter, i, j, l, g, gp, tc, ja, ia
    real(8) :: k, err, dr, rvp
    logical :: iflog
    type (strcpin), pointer :: ppin
!    ifwldt = .false.
    iflog  = .true.
    k=1._8
    do i=1, nxy
        do g=1, ng2
            phi(g,i,1) = 1._8 !drand(0)
        end do
    end do
    
    rvp=1._8/vpin
    htot=0;hrmv=0;hnuf=0;hmsct=0;
    do j=1, ny;ja=f2cy(j);
    do i=1, nx;ia=f2cx(i);
    if (map_exp2(ia,ja)  .eq. -1) cycle
    ppin=>typepin(cplst(i,j))
    do g=1, ng2
        do l=0, ppin%nr
            tc=ppin%rc(l)
            htot(g,i,j)=htot(g,i,j)+ppin%rv(l)*xtot(tc,g)
            hrmv(g,i,j)=hrmv(g,i,j)+ppin%rv(l)*xrmv(tc,g)
            hnuf(g,i,j)=hnuf(g,i,j)+ppin%rv(l)*xnuf(tc,g)
            do gp=1, ng2
                hmsct(gp,g,i,j)=hmsct(gp,g,i,j)+ppin%rv(l)*xsctm(gp,g,tc)
            enddo 
        enddo
        htot(g,i,j)=htot(g,i,j)*rvp
        hrmv(g,i,j)=hrmv(g,i,j)*rvp
        hnuf(g,i,j)=hnuf(g,i,j)*rvp
        do gp=1, ng2
            hmsct(gp,g,i,j)=hmsct(gp,g,i,j)*rvp
        enddo 
        hdif(g,i,j)=1._8/3/htot(g,i,j)
    enddo 
    enddo
    enddo
    
    call caldtilde
    call makemt(.false.)
    iter=0
    call poweriter(k, iter, err, dr, .false., iflog)
    
    hphi=0;htot=0;htot=0;hnuf=0;hmsct=0;hdif=0
!    call poweriter(k, iter, err, dr, .true.)
endsubroutine

subroutine copypsi(buf)
    real(8), pointer, dimension(:,:) :: buf
    integer :: k, l
    do k=1,nz;do l=1, nxy
        buf(l,k)=psi(l,k)
    enddo;enddo
endsubroutine


subroutine cmfd2g4mg(k, iter, err, phi_adj)
    include "pspec.FI"
    include "arrays.FI"
    include "hmgz.FI"
    integer :: iter, i, j, l, g
    real(8), intent(inout) :: k, err
    real(8) :: dr, rm
    real(8), pointer, dimension(:,:,:) :: phi_adj
    ! poting
    dif=>hdif2
    rmv=>hrmv2
    nuf=>hnuf2
    sct=>hmsct2
    mcphi=>hphi2
    mcj=>hj2
    
    call caldtilde
    call caldhat
    phi(:,1:,1:) = mcphi
    if (l1gcmfd) then ! the one-group CMFD
        call makemt1g(.true.)
    else
        call makemt(.true.)
    endif
!    err = 1e-3_8
    !err=err_out;
	iter=5
    call poweriter(k, iter, err, dr, .true., .false.)
    print *, "CMFD2G", k, iter, err
    rm = sum(mcphi)/sum(phi)
    l=0
    do j=1, ny
    do i=1, nx
        l=l+1
        do g=1, ng2
            if (phi(g,l,1)>0) phi_adj(g,l,1) = phi(g,l,1)/mcphi(g,i,j)*rm
        enddo
    enddo
    enddo
endsubroutine

subroutine cmfd2gacc(k_in, err, iter, err_cmfd, err_max, onfdb, powout)
    include "pspec.FI"
    include "arrays.FI"
    include "hmgz.FI"
    include "statistics.FI"
!    ! for debugging ========================
!    include "hmgz.FI"
!    ! ======================================
    integer :: iter
    real(8), intent(inout) :: k_in
    real(8) :: k, err, dr, sumadj
    real(8), intent(out) :: err_cmfd, err_max
    logical, intent(in) :: onfdb
    real(8), dimension(:,:) :: powout
    real(8) :: errloc
    real(8) :: upper, lower
    logical :: ifdhat, iflog
    integer :: i,j,l, g, s
    integer :: nfispin, nzero, nzeros
    real(8) :: nrmf, tpsi
!    integer, save  :: cmfdcount =0
!    real(8) :: htotmod

    ! discard early tallies for the last CMFD linear system 
    ! to obtain better solution of CMFD & P2B factor.    
!    cmfdcount=cmfdcount+1
!    if (CMFDDISCARD) then
!        if (cmfdcount .eq. ndscrd ) then
!            hjds=hj
!            hphids=hphi
!            htotds=htot
!            hRmvds=hRmv
!            hNuFds=hNuF
!            hmsctds=hmsct
!            print*, "ACC", sum(hphi)
!        elseif (cmfdcount .eq. (inactive+active)) then
!            hj=hj-hjds
!            hphi=hphi-hphids
!            hRmv=hRmv-hRmvds
!            hNuF=hNuF-hNuFds
!            hmsct=hmsct-hmsctds
!            do g=1,2; do j=1, ncmy; do i=1, ncmx
!                htotmod = (htot(g,i,j)-htotds(g,i,j))
!                if (htotmod>0) then
!                    hdif(g,i,j)=1._8/3/htotmod
!                else
!                    hdif(g,i,j)=0;
!                endif
!            enddo;enddo;enddo
!        endif
!    endif
    
    
    dif=>hdif;rmv=>hrmv;nuf=>hnuf;sct=>hmsct;mcphi=>hphi;mcj=>hj;    
    ifdhat = .true.
    iflog  = .false.
    
    
    
    k=k_in*1.1_8
    dr =0;
!    phi=1.
    nzero=0
    nzeros=0
    do g=1, ng2
        if (l1gcmfd .and. g>1) exit
        l=0;
        do j=1, ny
            do i=1, nx
                l=l+1
                phi(g,l,1)=hphi(g,i,j)
                if (map_exp2(i,j) .eq. -1) cycle
                if (hphi(g,i,j) == 0._8) then
                    dif(g,i,j)=1._8/(3*htot2(g,i,j))
                    nuf(g,i,j)=hnuf2(g,i,j)
                    rmv(g,i,j)=hRmv2(g,i,j)
                    sct(:,g,i,j)=hmsct2(:,g,i,j)
                    vacmesh(g,i,j,1)=.true.
                    nzero=nzero+1
                else
                    vacmesh(g,i,j,1)=.false.
                endif
                
                do k=1, 4
                    if (mcj(k,g,i,j) ==0._8) nzeros=nzeros+1
                enddo
!                noj(:,g,i,j,1) = .false.
!                do s=1, 4
!                    if (mcj(s,g,i,j) .eq. 0._8) noj(s,g,i,j,1)=.true.
!                enddo
            enddo
        enddo
!        if (hphi(g,i,j) > 0._8) then
    enddo
!    if (nzero >0) then
!        print *, "OH MY GOD", nzero
!!!        return
!    endif
    
    iter=0
    err =0
    if (nzero>0) then 
        fdbsafe = .false.
!        phi(:,i, 1) = 1._8
        k=0
    else
        call caldtilde
        call caldhat
        if (l1gcmfd) then ! the one-group CMFD
            call makemt1g(ifdhat)
        else
    !        call makemt(.false.)
            call makemt(ifdhat)
        endif

        call poweriter(k, iter, err, dr, ifwldt, iflog)
        
        
        if (ifwldt .eq. .false.) print *, dr
        ! check the solution
        nfispin=0
        do l=1, nxy
            if ( psi(l,1)<0) then
            
!                print *, "negative flux"
                iter = 0
                err  = 1.0_8
                k_IN=0
                do i=1, nxy
                    phi(:,i, 1) = 1._8
                enddo
                fdbsafe = .false.
                return
            elseif (psi(l,1)>0) then
                nfispin = nfispin+1
            endif
        enddo
    endif
    write(5001, 501), k, dr, nzero, nzeros, l*2, (1.0_8- 1.0_8*nzero/(l*2))*100
501 format (f7.5, 1x, es11.5, 1x, i10, 1x, i10, 1x, i10, 1x, f7.3)
    
    
    ! error check 
    nrmf =1._8/sum(psi)
    psi=psi*nrmf

    psiacc=psiacc+psi
    nrmf = 1._8/sum(psiacc)
    
    upper=0;lower=0;
    err_max=0
    if (ireft .eq. 1) then
        if (hybridcmfd) then
            do l=1, nxy
                upper = upper + (refcmfd(l)-psiacc(l,1)*nrmf)**2
                if (refcmfd(l)>0) then
                    errloc=abs(refcmfd(l)-psiacc(l,1)*nrmf)/refcmfd(l)
                    if(errloc>err_max) err_max=errloc
                endif
            enddo
        else
            do l=1, nxy
                upper = upper + (refcmfd(l)-psi(l,1))**2
    !            lower = lower + refcmfd(l)
                if (refcmfd(l)>0) then
                    errloc=abs(refcmfd(l)-psi(l,1))/refcmfd(l)
                    if(errloc>err_max) err_max=errloc
                endif
            enddo
        endif
        lower=1
    else
    !    nfispin=0;
        do l=1, nxy
            upper = upper + (psi(l,1)-psip(l,1))**2
            lower = lower + psi(l,1)
            if (psi(l,1)>0) then
                errloc=abs(psi(l,1)-psip(l,1))/psi(l,1)
                if(errloc>err_max) err_max=errloc
            endif
    !        nfispin = nfispin+1
        enddo
        psip=psi
    endif
    do j=1, ny
        powout(:,j)=psi((j-1)*ny+1:j*ny,1)
    enddo
    
    
    err_cmfd=sqrt(upper*npowasmref)/lower !*sqrt(1.0_8*nfispin)
    fdbsafe = .true.

    nrmf=nfispin
    psi = psi * nrmf *vcm
    phi = phi * nrmf
    

  
     
    k_in = k
    k_eff = k
    l = 0
    do j=1,ny
    do i=1, nx
#ifndef DBGLSV    
        if (map_exp2(i,j) .eq. -1) then ! for vacuum assembly
            adjp(i,j) = 0._8
        else
#endif
            l = l+1
            adjp(i,j)=psi(l,1)
            sumadj = sumadj + adjp(i,j)
!           fission weight map
            if (psi(l,1)>0) fiswtmap(i,j)=1._8/psi(l,1)
#ifndef DBGLSV    
        endif
#endif
    enddo
    enddo
    
!    adjp =1
!    fiswtmap =1
    
    do j=1,ny
    do i=1, nx
        adjp(i,j) = adjp(i,j)/sumadj
    enddo
    enddo

    if (unifsrc .and. onfdb) then
        psi=psi*nfispin/sum(psi)
        l=0
        do j=1, ncmy;do i=1, ncmx
            l=l+1
            pwadj(i,j) =1._8/psi(l,1)
            pwadjr(i,j)=psi(l,1)
        enddo;enddo
    endif
    
!    if (hybridcmfd ) then
!        do i=1, nx*ny
!            tpsi=psi(l,1)
!            stpsia(l,1)=stpsia(l,1)+tpsi
!            stpsiasq(l,1)=stpsiasq(l,1)+tpsi**2
!            l=l+1
!        enddo
!    endif
    
!    do j=1,ny
!        adjp(:,j)=psi((j-1)*nx+1: j*nx,1)
!    end do
!    adjp = adjp/sum(adjp)    
endsubroutine

subroutine caldtilde
    use define
    implicit none
    
    include "xsec.FI"
    include "pspec.FI"
    include "arrays.FI"
    include "hmgz.FI"
    
    integer i, j, k, g, oi, oj, bt
    real(8)    betaL, betaR, compL, compR
    Dtilde = 0.
    do g=1, ng2
        do j=1, ny
            do i=1, nx
                if (map_exp2(i,j) .eq. -1) cycle
                do k=1, 4
                    call neighc2(i,j,k,oi,oj)
!                    if (map_exp2(oi,oj) .eq. -1) cycle
                    if (map_exp2(oi,oj) .eq.-1) then ! for vacuum assembly
                        Dtilde(k,g,i,j) = Dbndry_vacuum(i,j,k,g) 
                    else
                        bt = bndryc(oi,oj)
                        if (bt>0)then
                            Dtilde(k,g,i,j)=Dbndry(i,j,k,g)
                        else
                            Dtilde(k,g,i,j)=Dinner(i,j,oi,oj,g)
                        endif
                    endif
                enddo
            enddo 
        enddo
    enddo
contains
function Dinner(mi,mj, oi,oj, g)
    integer :: mi, mj, oi, oj, g
    real(8) :: Dinner
    real(8) :: betaL, betaR

    Dinner = 0._8
    if (mcphi(g,mi,mj) .eq. 0._8 .or. mcphi(g,oi,oj) .eq. 0._8) then
        Dinner=0
    else
        if(mi .ne. oi) then ! x-dir
            betaL = dif(g,mi,mj)/hcmx
            betaR = dif(g,oi,oj)/hcmx
        else              
            betaL = dif(g,mi,mj)/hcmy
            betaR = dif(g,oi,oj)/hcmy
        endif
!        if (betaL >0 .and. betaR >0) then
            Dinner = 2*betaL*betaR/(betaL+betaR)
!        else    
!            Dinner = 0
!        endif
    endif
end function
function Dbndry(mi,mj,dr,g)
    integer :: mi, mj, dr,g
    real(8) :: Dbndry, betaL
    Dbndry = 0._8
    if (mcphi(g,mi,mj) .eq. 0) then
        Dbndry=0
    else
        if (dr>2) then
            betaL = dif(g,mi,mj)/hcmx
        else              
            betaL = dif(g,mi,mj)/hcmy
        endif
        Dbndry= 2*albedo(dr)*betaL/(albedo(dr)+2*betaL)
    endif
end function 

function Dbndry_vacuum(mi,mj,dr,g)
    integer :: mi, mj, dr,g
    real(8) :: Dbndry_vacuum, betaL, al
    al = 0.5_8
    if (dr>2) then
        betaL = dif(g,mi,mj)/hcmx
    else              
        betaL = dif(g,mi,mj)/hcmy
    endif
    Dbndry_vacuum= 2*al*betaL/(al+2*betaL)
end function 
end subroutine


subroutine caldhat 
    use define
    implicit none
    include "xsec.FI"
    include "pspec.FI"
    include "arrays.FI"
    include "hmgz.FI"
    integer :: i, j, k, g, oi,oj, mi, mj
    real(8) :: bt
    real(8) :: hcm(4)
    hcm(1:2) = hcmx
    hcm(3:4) = hcmy

    Dhat = 0.
    do g=1, ng2    
        do j=1, ny
            do i=1, nx
                if (mcphi(g,i,j) .eq. 0._8) cycle
                if (vacmesh(g, i,j,1)) then
                    dhat(:,g,i,j) = 0._8
                    cycle
                endif
                do k=1, 4
                    call neighc2(i,j,k,oi,oj)
                    if (map_exp2(oi,oj) .eq.-1) then ! for vacuum assembly
                        Dhat(k,g,i,j) = Dhatbndry(i,j,k,g) 
                    else
                        bt = bndryc(oi,oj)
                        if (bt>0)then
                            Dhat(k,g,i,j) = Dhatbndry(i,j,k,g)
                        else
                            if (vacmesh(g, oi,oj,1)) then
                                Dhat(k,g,i,j)=0._8
                            else
                                Dhat(k,g,i,j) = Dhatinner(i,j,oi,oj,k,g)
                            endif
                        endif
                    endif
                end do
            end do 
        end do
    end do
contains
function Dhatinner(mi,mj, oi,oj, k, g)
    include "hmgz.FI"
    integer :: mi, mj, oi, oj, k, g
    real(8) :: Dhatinner
    real(8) :: netj
    netj = mcj(k,g,mi,mj)
    if (netj .ne. 0._8) then
        Dhatinner = -( netj + Dtilde(k,g,mi,mj)*(mcphi(g,oi,oj)-mcphi(g,mi,mj)) )/(mcphi(g,mi,mj)+mcphi(g,oi,oj))
    else
        Dhatinner = 0._8
    endif
end function

function Dhatbndry(mi,mj,dr,g)
    include "hmgz.FI"
    integer :: mi, mj, dr, g
    real(8) :: Dhatbndry, betaL
    Dhatbndry = 0._8
    if (mcj(k,g,mi,mj) .ne. 0._8) then
        Dhatbndry = mcj(k,g,mi,mj)/mcphi(g,mi,mj)
    else
        Dhatbndry = albedo(dr)
    endif
end function 
end subroutine

subroutine makemt(ldh)
    use const, only :indm24
    use define
!    use geom
    implicit none    
    
    include "pspec.FI"
    include "hmgz.FI"
    
    logical, intent(in) :: ldh
    integer :: i, j, k, l, lo, g, gp, dr, d, nxs, nxe
    real(8) ::offsum
    real(8) :: hcm(4)
   
    hcm(1:2) = hcmx
    hcm(3:4) = hcmy
    do k=1, 1
    l=0
    do j=1, ny
        nxs = 0
        nxe = 1
        do i=1, nx
#ifdef DBGLSV
            l=l+1
#endif        
            if (map_exp2(i,j) .eq. -1) cycle ! for vacuum assembly
#ifndef DBGLSV
            l=l+1
#endif        
            ! check the first and the last mesh in a row
            if (nxs .eq. 0)  nxs = i
            nxe = i
            ! increment of real mesh index
            ! map for inner solver
            call setnodel(mtcmfd,i,j,l)
            !call setnxse(mtcmfd,j,1,nx)
            do g=1, ng2
                call setmb2g(mtcmfd,l,k,0,indm24(g,g),    rmv(g,i,j)*vcm)
                do dr=1, 4
                    ! neighbor
                    lo = neighc(i,j,dr)
                    if (ldh) then ! with Dhat
                        if (lo .eq. 0) then
                            call accmb2g(mtcmfd,l,k,0,indm24(g,g),    dhat(dr,g,i,j)*hcm(dr))
                        else
                            call accmb2g(mtcmfd,l,k,0,indm24(g,g),    (dtilde(dr,g,i,j)-dhat(dr,g,i,j))*hcm(dr))
                            call setmb2g(mtcmfd,l,k,dr,g,            -(dtilde(dr,g,i,j)+dhat(dr,g,i,j))*hcm(dr),lo)
                        endif
                    else    ! without Dhat
                        if (lo .eq. 0) then
                            call accmb2g(mtcmfd,l,k,0,indm24(g,g),    dtilde(dr,g,i,j)*hcm(dr))
                        else
                            call accmb2g(mtcmfd,l,k,0,indm24(g,g),    (dtilde(dr,g,i,j))*hcm(dr))
                            call setmb2g(mtcmfd,l,k,dr,g,            -(dtilde(dr,g,i,j))*hcm(dr),lo)
                        endif
                    endif
                enddo
            enddo
            ! group coupling
            call setmb2g(mtcmfd,l,k,0,indm24(1,2),-sct(1, 2, i,j)*vcm)
            call setmb2g(mtcmfd,l,k,0,indm24(2,1),-sct(2, 1, i,j)*vcm)
        enddo ! loop for x-direction
        ! set the real meshes in a row
        call setnxse(mtcmfd,j,nxs,nxe) 
    enddo ! loop for j-directoin
    enddo ! loop for plane
    
    call copymb2g(mtcmfdorg, mtcmfd)
    call facilu2g(mtcmfd)
end subroutine

subroutine makemt1g(ldh)
    use const, only :indm24
    use define
!    use geom
    implicit none    
    
    include "pspec.FI"
    include "hmgz.FI"
    
    logical, intent(in) :: ldh
    integer :: i, j, k, l, lo, g, gp, dr, d, nxs, nxe
    real(8) ::offsum
    real(8) :: hcm(4)
   
    hcm(1:2) = hcmx
    hcm(3:4) = hcmy
    do k=1, 1
    l=0
    do j=1, ny
        nxs = 0
        nxe = 1
        do i=1, nx
#ifdef DBGLSV
            l=l+1
#endif        
            if (map_exp2(i,j) .eq. -1) cycle ! for vacuum assembly
#ifndef DBGLSV
            l=l+1
#endif        
            ! check the first and the last mesh in a row
            if (nxs .eq. 0)  nxs = i
            nxe = i
            ! increment of real mesh index
            ! map for inner solver
            call setnodel(mtcmfd,i,j,l)
            !call setnxse(mtcmfd,j,1,nx)
            do g=1, ng2
                call setmb2g(mtcmfd,l,k,0,indm24(g,g),    rmv(1,i,j)*vcm)
                do dr=1, 4
                    ! neighbor
                    lo = neighc(i,j,dr)
                    if (ldh) then ! with Dhat
                        if (lo .eq. 0) then
                            call accmb2g(mtcmfd,l,k,0,indm24(g,g),    dhat(dr,1,i,j)*hcm(dr))
                        else
                            call accmb2g(mtcmfd,l,k,0,indm24(g,g),    (dtilde(dr,1,i,j)-dhat(dr,1,i,j))*hcm(dr))
                            call setmb2g(mtcmfd,l,k,dr,g,            -(dtilde(dr,1,i,j)+dhat(dr,1,i,j))*hcm(dr),lo)
                        endif
                    else    ! without Dhat
                        if (lo .eq. 0) then
                            call accmb2g(mtcmfd,l,k,0,indm24(g,g),    dtilde(dr,1,i,j)*hcm(dr))
                        else
                            call accmb2g(mtcmfd,l,k,0,indm24(g,g),    (dtilde(dr,1,i,j))*hcm(dr))
                            call setmb2g(mtcmfd,l,k,dr,g,            -(dtilde(dr,1,i,j))*hcm(dr),lo)
                        endif
                    endif
                enddo
            enddo
        enddo ! loop for x-direction
        ! set the real meshes in a row
        call setnxse(mtcmfd,j,nxs,nxe) 
    enddo ! loop for j-directoin
    enddo ! loop for plane
    
    call copymb2g(mtcmfdorg, mtcmfd)
    call facilu2g(mtcmfd)
!    call print_mt
CONTAINS
subroutine print_mt
    integer, parameter :: dbgf = 1010
    integer :: z
    open(dbgf, file="mt.txt", status='unknown')
    g=1;z=1
    l=0
    do j=1, ny
        do i=1, nx
            l=l+1
            write(dbgf, 1011) l, mtcmfd%am(g,l,z), mtcmfd%iccr(1,l,z), mtcmfd%ccr(1,g,l,z), mtcmfd%iccr(2,l,z), mtcmfd%ccr(2,g,l,z), mtcmfd%iccr(3,l,z), mtcmfd%ccr(3,g,l,z), mtcmfd%iccr(4,l,z), mtcmfd%ccr(4,g,l,z)
1011 format (i6,1x, es11.5,1x, i6,1x, es11.3,1x, i6,1x, es11.3,1x, i6,1x, es11.3,1x, i6,1x, es11.3,1x)        
        enddo
    enddo
    close(dbgf)
endsubroutine    
end subroutine

subroutine shiftmt(ke)
    use const, only :indm24
    implicit none 
    include "hmgz.FI"
    include "xsec.FI"
    include "pspec.FI"
    real(8), intent(in) :: ke
    real(8)             :: ker
    integer             :: k,j,i,l
    
    ker = 1._8/ke
    
    call copymb2g(mtcmfd, mtcmfdorg)
    l=0
    do k=1, nz
    do j=1, ny
        do i=1, nx
#ifdef DBGLSV
            l=l+1
#endif        
            if (map_exp2(i,j) .eq. -1) cycle ! for vacuum assembly
#ifndef DBGLSV
            l=l+1
#endif        
            ! update diagonal entries 
            call accmb2g(mtcmfd,l,k,0,indm24(1,1), -ker*nuf(1,i,j)*vcm)
            call accmb2g(mtcmfd,l,k,0,indm24(1,2), -ker*nuf(2,i,j)*vcm)
!            call accmb2g(mtcmfd,l,k,0,indm24(1,1), -ker*Xchi(1,1)*nuf(1,i,j)*vcm)
!            call accmb2g(mtcmfd,l,k,0,indm24(2,2), -ker*Xchi(1,2)*nuf(2,i,j)*vcm)
!            call accmb2g(mtcmfd,l,k,0,indm24(1,2), -ker*Xchi(1,1)*nuf(2,i,j)*vcm)
!            call accmb2g(mtcmfd,l,k,0,indm24(2,1), -ker*Xchi(1,2)*nuf(1,i,j)*vcm)
        enddo
    enddo
    enddo
endsubroutine


subroutine cmfd2gFS
!    use geom
    include "xsec.FI"
    include "hmgz.FI"
    include "pspec.FI"
    integer :: i,j,l,g
    real(8) :: lpsi
    l=0
    do j=1, ny
        do i=1, nx
#ifdef DBGLSV
            l=l+1
#endif        
            if (map_exp2(i,j) .eq. -1) then
                psi(l,1)=0._8
                cycle ! for vacuum assembly
            endif
#ifndef DBGLSV
            l=l+1
#endif        
            lpsi=0
            do g=1, ng2
                lpsi=lpsi+nuf(g,i,j)*phi(g,l,1)
            enddo
            psi(l,1) = lpsi*vcm
        enddo
    enddo
endsubroutine

subroutine cmfd2gQ(k,q)
    include "pspec.FI"
    include "arrays.FI"
    include "xsec.FI"
    real(8), intent(in) :: k
    real(8), dimension(:,:,:), intent(out):: q
    integer :: i, j, g, l, gp
    q=0
    l=0
    do j=1, ny
        do i=1, nx
#ifdef DBGLSV
            l=l+1
#endif        
            if (map_exp2(i,j) .eq. -1) cycle ! for vacuum assembly
#ifndef DBGLSV
            l=l+1
#endif        
!            do g=1, ng2
!                q(g,l,1) = Xchi(1,g)*psi(l,1)/k
!            end do
            do g=1, 1
                q(g,l,1) = psi(l,1)/k
            end do
        end do 
    end do
endsubroutine

subroutine check_mt
!    include "pspec.FI"
!    include "arrays.FI"
    include "xsec.FI"
    integer :: i, j, k, g, l
    real(8) :: tsum
    
    do j=1, ny
    do i=mtcmfd%nxs(j), mtcmfd%nxe(j)
        l=(j-1)*nx+i
        tsum = sum(mtcmfd%am(:,l,1))
        if (tsum .eq. 0 ) then
            print *, "diag", i, j
        endif
    enddo
    enddo
endsubroutine

subroutine poweriter(k, iter, err, dr, ifwldt, iflog)
    use bicg2g, only : initbicg2g, solbicg2g
    use ifport
!$  use omp_lib
    implicit none

    real(8), dimension(nxy,nz)      :: psi_prv
    real(8), dimension(ng2,nxy,nz)   :: q
    real(8), intent(inout)          :: k, err
    real(8), intent(out)            :: dr
    integer, intent(inout)            :: iter
    integer                         :: i, j, g, gp, maxiterin, max_iter, iti, tot_in, iterconv
    real(8)                         :: errconvin, torErr, upper, upper2, lower, err_prv, ke, kd, kr, ERRCONV
    real(8)                         :: r2, r20
    logical                         :: ifwldt, iflog
    integer :: iterout
    real(8) :: errout
    
                       
!$  real(8) :: ts(3)
!$  ts(3) = 0
!$      ts(1) = omp_get_wtime()

	if (err .ne. 0._8) then
		errconv = err
	else
		errconv = ccout
	endif
	if (iter .ne. 0) then
		iterconv = iter
	else
		iterconv = maxout
	endif
    err    = 1.
	iter   =0

!    iterout=iter
!    errout=err
!    if(iterout .eq. 0 .or. errout .eq. 0) then
!        iterout=maxout
!        errout =ccout
!    endif

!    max_iter=iter
    iter   = 0
    err    = 1.
    dr     = 1.
    if (ifwldt) kr=k
    tot_in=0                                
    ! make fission source
    call cmfd2gFS
    
    call check_mt()

    do while (iter .lt. iterconv .and. err.gt.errconv)
        err_prv = err
        iter = iter + 1
        if (ifwldt) then
            kd=shiftk(iter, 0.01_8)
            ke=kr+kd
            k = 1._8/(1._8/kr-1._8/ke)
            call shiftmt(ke)
        endif
        ! make total source
        call cmfd2gQ(k,q)
!        print *, sum(q), sum(mtcmfd%am), sum(mtcmfd%ccr)
        call initbicg2g(mtcmfd,phi,q,r20)
!        print *, iter, r20
        if (r20<1e-10) exit
        r2=r20;iti=0;
!        do while (r2>1e-3)  ! i=1,3
        do while (iti .lt. maxin .and. r2/r20.gt.ccin)
!        do while (iti .lt. 20)
            call solbicg2g(mtcmfd,r20,r2,phi)
!            print *, iter, iti, r2
            iti=iti+1
        enddo
        tot_in = tot_in+iti
!        write(*,*) r20, r2, iti
            
        ! make fission source from updated flux
        psi_prv = psi
        call cmfd2gFS
                
        ! evaluate k                                            
        upper = 0.
        lower = 0.
        upper2= 0.
!!$omp parallel do private(i), reduction(+:upper,lower,upper2)       
        do i=1, nxy                                                
            upper = upper + psi(i,1)*psi(i,1)
            lower = lower + psi(i,1)*psi_prv(i,1)
            upper2 = upper2 + (psi(i,1)- psi_prv(i,1))*(psi(i,1)- psi_prv(i,1))
        end do
!!$omp end parallel do
        if(ifwldt) then
            k=k*upper/lower
            kr=(k*ke)/(k+ke)
        else
            k=k*upper/lower
        endif        
                
        ! evaluate err 
        lower = sqrt(upper)
        upper = sqrt(upper2)
        err = upper/lower
    end do
!$      ts(2) = omp_get_wtime()
    if (ifwldt) k=kr
    dr = err/err_prv  
    if(iflog) then
        write(*,*) iter, err, tot_in
        write(*, '(" Solution from FDM : k=",f7.5,1x,"dr=",f7.5)'), k, dr
        write(*, '(" Computing time    : ", f12.3,1x,"(sec)")') ts(2)-ts(1)
    endif
    
    contains
    
function shiftk(iter, delk) result (ret)
!    real(8) :: SHIFT_GUIDE(17)
!    data SHIFT_GUIDE /1.0_8, 0.75_8, 0.50_8, 0.35_8, 0.25_8, &
!                    0.20_8, 0.15_8, 0.125_8, 0.100_8, 0.075_8, 0.050_8, &
!                    0.035_8, 0.025_8, 0.020_8, 0.015_8, 0.0125_8, 0.0100_8/
    real(8) :: SHIFT_GUIDE(7)
    data SHIFT_GUIDE /1.0_8, 0.50_8, 0.25_8, 0.125_8, 0.075_8, 0.035_8, 0.01_8/
    integer, intent(in) :: iter
    real(8), intent(in) :: delk
    real(8) :: ret
    ret = delk
    if (iter .le. 7) then
        if (ret<SHIFT_GUIDE(iter)) ret = SHIFT_GUIDE(iter) 
    endif
end function
    
end subroutine


endmodule
